import lejos.nxt.*;
import lejos.nxt.addon.GyroSensor;

/**
 *
 * @author Pawel Jankowski
 */
public class Gyroscope {

    private SensorPort sensorPort;
    private double angle = 0;
    private double angularVelocity = 0;
    private double offset = (double) 611.5;
    private int lastCall = 0;
    private GyroSensor sensor;
    

    public Gyroscope(SensorPort s) {
        this.sensorPort = s;
        this.sensor = new GyroSensor(sensorPort);
        sensor.setOffset((int) offset);
        //this.calibrate();
    }

    public GyroscopeData getData()
    {

        int now = (int) System.currentTimeMillis();
        int difference = now - lastCall;

        this.angularVelocity = (double) this.sensorPort.readRawValue() - this.offset;

        if (difference != now ) {
            this.angle += this.angularVelocity * ((double) difference / 1000.0);
        }

        GyroscopeData gyroscopeData = new GyroscopeData((int) this.angle, (int) this.angularVelocity);
        
        lastCall = now;

        return gyroscopeData;
    }

   public void calibrate()
   {
      this.offset = 0;
      double offsetTotal = 0;


      for (int i = 0; i < 20; i++) {
          offsetTotal += (double) this.sensorPort.readRawValue();
          System.out.println(i);
      }
      this.offset = Math.floor(offsetTotal / 20);

      try {
         Thread.sleep(3000);
      } catch (InterruptedException e) {
      }
   }

    public void resetAngle(){
        this.angle = 0;
    }
}
